#ifndef __CONTROL__H
#define __CONTROL__H
#include "pid.h"
#include "line.h"
#include "opencv.hpp"
using namespace cv;

const float Forward_Quick = 0.8f;
const float Forward_Slow = 0.4f;
const float Stop = 0.0f;

class RobotController
{
public:
  typedef enum
  {
    Forward = 0, // 正常寻迹模式
    Obstacle,    // 避障模式
    Horizontal,  // 平移模式
    LoseLine,    // 丢线模式
  } Mode_t;      // 四足的运动模式

  typedef enum
  {
    Mid = 0,    // 在盲道上
    Left,       // 在盲道左边
    Right,      // 在盲道右边
  } Position_t; // 四足相对于盲道的位置

  RobotController();
  ~RobotController() {}

  Line midLine, leftLine, rightLine;
  PID yaw_pid = PID(0, 0, 0);   //输入为midline的夹角
  PID side_pid = PID(0, 0, 0);  //输入为midline的中点横坐标
  Mode_t mode;
  Position_t position;
  float forward_speed = 0; //取值范围:-1~1,对应 -0.7~1 (m/s)
  float side_speed = 0;    //取值范围:-1~1,对应 -0.4~0.4 (m/s). 小于0向右
  float yaw_speed = 0;     //(rad/s) 大于0是逆时针,小于0是顺时针

public:
  void get_Line(Mat &pred, Mat &H);
  void calculate_speed(void);
};

#endif
